#include "control.h"
#include "element.h"

#include "ssd1327.h"
#include "led.h"
#include "motor.h"
#include "encode.h"
#include "adc128s.h"
#include "imu.h"
#include "buzz.h"

#include "vl53l0x.h"
#include "mymath.h"
#include "pid.h"

#include "dma.h"
#include "zf_delay.h"
#include "zf_eeprom.h"
#include <string.h>

// Aa/Bb增强直道两个电感的作用，增大Bb/Cc提高车“切弯道倾向”
Ctrl_type Ctrl;
// 小车全局速度
int16 CarVelocity = 150;

// 速度闭环PID
pid_type_def SpeedLoop;
const static fp32 SpeedLoop_PID[3] = {12, 1, 3};
// 角度闭环PID
pid_type_def AngleLoop;
const static fp32 AngleLoop_PID[3] = {7.4, 0, 0};
// 角速度闭环PID
pid_type_def PalstanceLoop;
const static fp32 PalstanceLoop_PID[3] = {900, 0, 0};
// 俯仰角闭环PID，提供阻尼
pid_type_def PitchLoop;
const static fp32 PitchLoop_PID[3] = {1, 0, 0};
// 元素响应闭环PID
pid_type_def EleResLoop;
// const static fp32 EleResLoop_PID[3] = {-0.14, -0.0003, 0};//路障好用
// const static fp32 EleResLoop_PID[3] = {-0.08, 0, 0};//停车好用
// const static fp32 EleResLoop_PID[3] = {-3.6, 0, 0};//记录好用
const static fp32 EleResLoop_PID[3] = {-0.2, 0, 0};

void Ctrl_Init()
{
    IMU_Init();
    Elem_Init();

    // 初始化差比和差加权数值
    // Ctrl.Aa = 0.4f;
    // Ctrl.Bb = 0.36f;
    // Ctrl.Cc = 0.14f;
    Ctrl.Aa = 0.3f;
    Ctrl.Bb = 0.3f;
    Ctrl.Cc = 0.1f;
    Ctrl.CcCopy = 100;
    // 默认关闭小车
    Ctrl.Car = DISABLE;
    Elem.Road = No_Elem;
    Ctrl.GarageInto = DISABLE;
    Ctrl.Velocity = CarVelocity;

    // PID各种环的初始化，差分pid的i没有限制
    // 速度闭环
    PID_init(&SpeedLoop, PID_POSITION, SpeedLoop_PID, MOTOR_MAX_PWM, MOTOR_MAX_PWM / 3);
    // 角度闭环
    PID_init(&AngleLoop, PID_POSITION, AngleLoop_PID, 180, 30);
    // 角速度闭环
    PID_init(&PalstanceLoop, PID_POSITION, PalstanceLoop_PID, MOTOR_MAX_PWM, MOTOR_MAX_PWM / 3);
    // 俯仰角闭环，提供阻尼
    PID_init(&PitchLoop, PID_POSITION, PitchLoop_PID, Ctrl.Velocity / 2, 0);
    // 元素响应闭环
    PID_init(&EleResLoop, PID_POSITION, EleResLoop_PID, 180, 30);
}

void Ctrl_Process()
{
    // 采集电感的值
    ADC_GetData();
    // 采集陀螺仪的值
    IMU_GyroOffsetCalc();
    IMU_Process();
    // 获取编码器值
    Enco_GetData();
    // 采集赛道记录，失败了，废案
    // Elem_Record();

    // Ctrl.IndErr = (Ctrl.Aa * (ADC.Std[0] - ADC.Std[6]) + Ctrl.Bb * (ADC.Std[1] - ADC.Std[5])) /
    //               (Ctrl.Aa * (ADC.Std[0] + ADC.Std[6]) + Ctrl.Cc * M_ABS(ADC.Std[1] - ADC.Std[5]));

    Ctrl.IndErr = (Ctrl.Aa * (ADC.Raw[1] - ADC.Raw[7]) + Ctrl.Bb * (ADC.Raw[2] - ADC.Raw[6])) /
                  (Ctrl.Aa * (ADC.Raw[1] + ADC.Raw[7]) + Ctrl.Cc * M_ABS(ADC.Raw[2] - ADC.Raw[6]));
    // 夏普红外测距传感器20到150，小于20就不可靠了
    Elem.Distance = 22.3288f * ADC.InfVol * ADC.InfVol * ADC.InfVol * ADC.InfVol - 166.4483f * ADC.InfVol * ADC.InfVol * ADC.InfVol + 457.5147f * ADC.InfVol * ADC.InfVol - 573.6003f * ADC.InfVol + 319.9617f;
    // 夏普红外测距传感器10到80，小于10就不可靠了
    // Elem.Distance = -6.3681f * ADC.InfVol * ADC.InfVol * ADC.InfVol * ADC.InfVol * ADC.InfVol + 62.1627f * ADC.InfVol * ADC.InfVol * ADC.InfVol * ADC.InfVol - 236.5428f * ADC.InfVol * ADC.InfVol * ADC.InfVol + 444.1693f * ADC.InfVol * ADC.InfVol - 427.7095f * ADC.InfVol + 192.0019f;

    if (Ctrl.Car == ENABLE)
    {
        if (Elem.Road == No_Elem)
        {
            // 元素识别
            Elem_Identify();
            // 动态调p，乘上系数可以改变趋势，5000是可以改的零点
            // Ctrl.DynamicAP = LIMIT_BP(((-ADC.Std[3] + 10000) / 2500.0f), 1, 0.6);
            Ctrl.DynamicVP = LIMIT_BP((ADC.Std[3] / 9000.0f), 1, 0.86f);
            // 切换为电磁赛道误差角度闭环
            PID_calc(&AngleLoop, Ctrl.IndErr, 0); // 越大越不需要调整 * Ctrl.DynamicAP
            PID_calc(&PalstanceLoop, IMU.RawGyro[z], AngleLoop.out);
            // 经过校准的pitch用来做阻尼，不然车子老低头就不用跑了
            // 错误的，因为车子在加减速的时候俯仰角会变化，所以无法判断坡道
            // PID_calc(&PitchLoop, IMU.EulerAngle[pitch], 0);
            PID_calc(&SpeedLoop, Enco.Velocity, Ctrl.Velocity * Ctrl.DynamicVP); // 越大越要加速
            Motor_SetPWM(-PalstanceLoop.out + SpeedLoop.out, PalstanceLoop.out + SpeedLoop.out);
            // 判断电感值大小，以确定是否出赛道；电池电压小于10v也停车，提醒充电；物块传感器没有finish也会停车，否则容易撞坏
            // !dl1a_finsh_flag不能加，不然可能跑不了
            if ((ADC.Raw[1] + ADC.Raw[7]) < 20 || ADC.BatVol < 10)
            {
                Ctrl.Car = DISABLE;
                IMU_SetYaw(0);
            }
        }
        else
        {
            // 元素响应
            Elem_Response();
            // 切换为IMU计算的偏航角角闭环
            PID_calc(&EleResLoop, IMU_CalcYawRef(), 0);
            PID_calc(&PalstanceLoop, IMU.RawGyro[z], EleResLoop.out);
            PID_calc(&SpeedLoop, Enco.Velocity, Ctrl.Velocity);
            Motor_SetPWM(-PalstanceLoop.out + SpeedLoop.out, PalstanceLoop.out + SpeedLoop.out);
        }
        // 负压风扇函数
        BLDC_SetSpeed(Brushless, Brushless);
    }
    else
    {
        if (Ctrl.GarageInto == ENABLE)
        {
            // 入库停车遵从角度闭环
            PID_calc(&EleResLoop, IMU_CalcYawRef(), 0);
            PID_calc(&PalstanceLoop, IMU.RawGyro[z], EleResLoop.out);
            // 设置速度为零
            PID_calc(&SpeedLoop, Enco.Velocity, 0);
            Motor_SetPWM(-PalstanceLoop.out + SpeedLoop.out, PalstanceLoop.out + SpeedLoop.out);
            BLDC_SetSpeed(0, 0);
        }
        else
        {
            // 设置速度为零
            PID_calc(&SpeedLoop, Enco.Velocity, 0);
            Motor_SetPWM(SpeedLoop.out, SpeedLoop.out);
            BLDC_SetSpeed(0, 0);
        }
    }
}

void Ctrl_CarStart(void)
{
    BLDC_SetSpeed(Brushless, Brushless);
    // 读取元素数据
    Elem.FlashRead = TRUE;
    Elem_E2PROM();
    delay_ms(1400);
    u8g2_ClearDisplay(&u8g2);                // 清空屏幕
    Ctrl.Car = ENABLE;                       // 开启小车
    Counter.Tim4 = 0;                        // 总计时清零
    IMU_Reset();                             // 重置角度
    Elem_Reset(ON);                          // 重置元素
    Ctrl.GarageInto = DISABLE;               // 关闭入库锁角度
    Elem.OutDirect = RightDirect;            // 设置出库方向
    Elem.Road = Start_And_Out_Of_The_Garage; // 启动并驶出车库
    Ctrl.Velocity = Response[Start_And_Out_Of_The_Garage][0].Speed;
    Elem.State = Response[Start_And_Out_Of_The_Garage][0].State;
    IMU_SetYaw(Response[Start_And_Out_Of_The_Garage][0].Angle);
}
